import math

class astar:
    def __init__(self):
        self.cities={}
        self.source_path=""
        self.cities_path={}
        self.heap=[]
        self.heap_md={}
    def add_city(self,name,x,y):
        self.cities[name]=(x,y,[])
        #print self.cities
    def add_conection(self,src,dst,cust):
        self.cities[src][2].append((dst,cust))
        #print self.cities
    def add_biconection(self,src,dst,cust):
        self.add_conection(src,dst,cust)
        self.add_conection(dst,src,cust)
    def short_value(self,src,dst):
        self.__short(src,dst)
        return self.cities_path[dst][0]
    def short_path(self,src,dst):
        path=[dst]
        self.__short(src,dst)
        prox=dst
        #print self.cities_path
        while prox!=src:
            prox=self.cities_path[prox][1]
            path[:0]=[prox] # naum tem nada mais bonito?
        return path
    def distancia(self,x1, x2, y1, y2):
        dx = x2 - x1
        dy = y2 - y1
        dquadrado = dx**2 + dy**2
        return math.sqrt(dquadrado)
    def __short(self,src,dst):
        if src!=self.source_path:
            self.heap=[]
            self.heap_md={}
            self.cities_path={}
            self.source_path=src
            self.cities_path[src]=(0,src)
            self.heap_push(src,0)
        while self.heap!=[]:
            c=self.heap_pop()
            if self.cities_path.has_key(dst) and c[1]>=self.cities_path[dst][0]:
                break
            print "Avaliando: ",c[0]
            for k in self.cities[c[0]][2]:
                if ( not self.cities_path.has_key(k[0])) or (self.cities_path[k[0]][0]>k[1]+self.cities_path[c[0]][0]):
                    self.cities_path[k[0]]=(k[1]+self.cities_path[c[0]][0],c[0])
                    self.heap_push(k[0],self.cities_path[k[0]][0]+self.distancia(self.cities[k[0]][0],self.cities[dst][0],self.cities[k[0]][1],self.cities[dst][1]))
        #print self.cities_path
    def heap_push(self,city,cust):
        if self.heap_md.has_key(city):
            self.heap[self.heap_md[city]][1]=cust
            self.heap_up(self.heap_md[city])
            self.heap_down(self.heap_md[city])
        else:
            self.heap.append((city,cust))
            self.heap_up(len(self.heap)-1)
    def heap_pop(self):
        ret=self.heap[0]
        top=self.heap.pop()
        del self.heap_md[ret[0]]
        if len(self.heap)>0:
            self.heap[0]=top
            self.heap_down(0)
        return ret
    def heap_down(self,pos):
        while 2*pos+1<len(self.heap):
            if(2*pos+2<len(self.heap)):
                if self.heap[pos*2+1][1]<self.heap[pos*2+2][1]:
                    if self.heap[pos][1]> self.heap[pos*2+1][1]:
                        temp=self.heap[pos*2+1]
                        self.heap[pos*2+1]=self.heap[pos]
                        self.heap[pos]=temp
                        pos=pos*2+1
                        continue
                    else:
                        break
                else:
                    if self.heap[pos][1]> self.heap[pos*2+2][1]:
                        temp=self.heap[pos*2+2]
                        self.heap[pos*2+2]=self.heap[pos]
                        self.heap[pos]=temp
                        pos=pos*2+2
                        continue
                    else:
                        break
            else:
                if self.heap[pos][1]> self.heap[pos*2+1][1]:
                    temp=self.heap[pos*2+1]
                    self.heap[pos*2+1]=self.heap[pos]
                    self.heap[pos]=temp
                    pos=pos*2+1
                    continue
                else:
                    break
        self.heap_md[heap[pos][0]]=self.heap[pos][1]
    
    def heap_up(self,pos):
        while pos>=0:
            if self.heap[pos][1]<self.heap[(pos-1)/2][1]:
                temp=self.heap[(pos-1)/2]
                self.heap[(pos-1)/2]=self.heap[pos]
                self.heap[pos]=temp
                pos=(pos-1)/2
            else:
                break
        self.heap_md[self.heap[pos][0]]=self.heap[pos][1]
                
                    
    def printf(self):
        print self.cities
        

a=astar();
a.add_city("salvador",0,0)
a.add_city("brasilia",0,-1000)
a.add_city("campinas",-500,-1000)
a.add_biconection("salvador","brasilia",2000)
a.add_biconection("campinas","brasilia",2000)
print a.short_value("salvador","campinas")
print a.short_path("salvador","campinas")

